#include "Physics.h"
#include <Windows.h>
#include <cassert>
#include <math3d.h>
#include "GameSettings.h"


namespace ImpossiblePhysics
{
	const float_t GRAVITY = 80.0f;

	// Physics handle to player's collider object (Used to set collision state)
	void* g_playerCollider = NULL;

	// CALLBACKS

	// Simulation tick callback function
	void PhysicsUpdateCallback(btDynamicsWorld* argWorld, btScalar timeStep)
	{
		// Check contact points
		uint32_t _numManifolds = argWorld->getDispatcher()->getNumManifolds();

		// If there are no manifolds, the player is not colliding with anything
		if(_numManifolds == 0)
		{
			((PhysicsObj*)g_playerCollider)->hit = false;
		}


		for(uint32_t i = 0; i < _numManifolds; ++i)
		{
			btPersistentManifold* _contactManifold = argWorld->getDispatcher()->getManifoldByIndexInternal(i);
			btCollisionObject* _collObjA = (btCollisionObject*)(_contactManifold->getBody0());
			btCollisionObject* _collObjB = (btCollisionObject*)(_contactManifold->getBody1());

			PhysicsObj* _playerObj = NULL;
			PhysicsObj* _environmentObj = NULL;
			btCollisionObject* _playerCollObj = NULL;
			btCollisionObject* _envCollObj    = NULL;
			PhysicsObj* _physObjA  = (PhysicsObj*)_collObjA->getUserPointer();
			PhysicsObj* _physObjB  = (PhysicsObj*)_collObjB->getUserPointer();

			// Assign player object
			if(_physObjA->idString == PLAYER_OBJECT)
			{
				_playerObj = _physObjA;
				_environmentObj = _physObjB;
				_playerCollObj = _collObjA;
				_envCollObj = _collObjB;
			}
			else if(_physObjB->idString == PLAYER_OBJECT)
			{
				_playerObj = _physObjB;
				_environmentObj = _physObjA;
				_playerCollObj = _collObjB;
				_envCollObj = _collObjA;
			}
			else
			{
				_playerObj = NULL;
				_playerCollObj = NULL;
				_envCollObj = NULL;
			}

			// If this collision does not involve the player, we don't care about it
			if(!_playerObj) continue;

			// DETERMINE IF THIS COLLISION WAS FROM THE TOP

			_playerObj->hit = false;

			int32_t _numContacts = _contactManifold->getNumContacts();

			for(int32_t j = 0; j < _numContacts; ++j)
			{
				btManifoldPoint& _pt = _contactManifold->getContactPoint(j);
				if(_pt.getDistance() < 0.0f)
				{
					btTransform _objBTransform   = _envCollObj->getWorldTransform();
					btVector3 o = _objBTransform.getOrigin();

					// Top of environment object
					btVector3 _halfWidthsB = ((btBoxShape*)_envCollObj->getCollisionShape())->getHalfExtentsWithMargin();
					o.setY(o.y() + _halfWidthsB.y());

					// Check if the player's bottom before was higher than the object's top
					if(_playerObj->bottom >= (o.y() - COLLISION_DEPTH_ERROR))
					{
						// Collision detected between player and environment
						_playerObj->hit = true; 
						// Store the depth of this collision object
						_playerObj->depth = _environmentObj->depth;


					}

					if(_environmentObj->idString == KILL_OBJECT)
					{
						_playerObj->dead = true;
					}
					//else
					//{
					//	// Death collision code here...
					//	int x = 0; // DEBUG STATEMENT
					//}

					break;
				}
			}
		}
	}

	// PHYSICS ENTITY IMPLEMENTATION

	PhysicsObj::physicsObject(btRigidBody* argRigidBody, std::string argID, float_t argr, float_t argg, float_t argb)
		: body(argRigidBody),
		  idString(argID),
		  r(argr),
		  g(argg),
		  b(argb),
		  hit(false),
		  dead(false),
		  top(0.0f),
		  bottom(0.0f),
		  depth(0.0f)
	{
	}

	PhysicsObj::physicsObject()
		: hit(false),
		  r(0.f),
		  g(0.f),
		  b(0.f),
		  dead(false),
		  top(0.0f),
		  bottom(0.0f),
		  depth(0.0f)
	{
	}

	// PHYSICS CLASS IMPLEMENTATION

	Physics::Physics()
		:
	m_fGravity(-GRAVITY)
	{
	}


	Physics::~Physics(void)
	{
		delete m_collisionConfig;
		delete m_dispatcher;
		delete m_broadphase;
		delete m_solver;
		delete m_world;
	}

	void Physics::Init()
	{
		m_collisionConfig = new btDefaultCollisionConfiguration();
		m_dispatcher      = new btCollisionDispatcher(m_collisionConfig);
		m_broadphase      = new btDbvtBroadphase();
		m_solver          = new btSequentialImpulseConstraintSolver();
		m_world           = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfig);

		// Set simulated tick callback
		m_world->setInternalTickCallback(ImpossiblePhysics::PhysicsUpdateCallback);
		// Gravity
		m_world->setGravity(btVector3(0,m_fGravity,0));
		// Set contact added callback (Detect collisions)
		//gContactAddedCallback = (ContactAddedCallback)PhysicsContactAddedCallback;
	}

	//You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
	void Physics::Update(float_t argDeltaTime)
	{
		m_world->stepSimulation(0.01666666754f, 0);
		//m_world->stepSimulation(argDeltaTime);
	}

	const btDynamicsWorld* const Physics::GetWorld() const
	{
		return m_world;
	}

	// INSPECT FOR MEMORY LEAK
	void Physics::AddBoxPlayer(PhysicsObj*& argPhysObj,
		float_t argPosX, float_t argPosY ,float_t argPosZ,
		float_t argHalfExtentX, float_t argHalfExtentY, float_t argHalfExtentZ,
		float_t argMass)
	{
		// This should already be allocated
		assert(argPhysObj);

		//btRigidBody* argRigidBody = argPhysObj->body;
		// Set up the transform
		btTransform _transform;
		_transform.setIdentity();
		_transform.setOrigin(btVector3(argPosX, argPosY, argPosZ));
		// Define the shape using the half extents
		btBoxShape* _boxShape = new btBoxShape(btVector3(argHalfExtentX, argHalfExtentY, argHalfExtentZ));
		// Set inertia
		btVector3 _inertia(0, 0, 0);
		_boxShape->calculateLocalInertia(argMass, _inertia);
		// Define default motion
		btMotionState* _motion = new btDefaultMotionState(_transform);
		// Construct rigid body
		btRigidBody::btRigidBodyConstructionInfo _info(argMass, _motion, _boxShape, _inertia);
		argPhysObj->body = new btRigidBody(_info);
		// Disable sleeping
		//argPhysObj->body->setActivationState(DISABLE_DEACTIVATION);
		argPhysObj->body->setActivationState(ISLAND_SLEEPING);
		// Set rotation factor
		//argPhysObj->body->setAngularFactor(Settings::PLAYER_ANGULAR_FACTOR);
		argPhysObj->body->setAngularFactor(0.0f);
	
		// Set collision flags
		argPhysObj->body->setCollisionFlags(argPhysObj->body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
		//argPhysObj
		// Set user pointer
		argPhysObj->body->setUserPointer(argPhysObj);
		// Add body to world
		m_world->addRigidBody(argPhysObj->body);
	}

	// INSPECT FOR MEMORY LEAK
	void Physics::AddBoxEnvironment(PhysicsObj*& argPhysObj,
		float_t argPosX, float_t argPosY ,float_t argPosZ,
		float_t argHalfExtentX, float_t argHalfExtentY, float_t argHalfExtentZ,
		float_t argMass, std::string argIdString)
	{
		assert(argPhysObj);

		// Set up the transform
		btTransform _transform;
		_transform.setIdentity();
		_transform.setOrigin(btVector3(argPosX, argPosY, argPosZ));
		// Define the shape using the half extents
		btBoxShape* _boxShape = new btBoxShape(btVector3(argHalfExtentX, argHalfExtentY, argHalfExtentZ));
		// Set inertia
		btVector3 _inertia(0, 0, 0);
		_boxShape->calculateLocalInertia(argMass, _inertia);
		// Define default motion
		btMotionState* _motion = new btDefaultMotionState(_transform);
		// Construct rigid body
		btRigidBody::btRigidBodyConstructionInfo _info(argMass, _motion, _boxShape, _inertia);
		argPhysObj->body = new btRigidBody(_info);

		// Store depth of this physics object
		argPhysObj->depth = argPosX;

		// Set id string
		argPhysObj->idString = argIdString;

		// Set user pointer
		argPhysObj->body->setUserPointer(argPhysObj);

		// Add body to world
		m_world->addRigidBody(argPhysObj->body);
	}

	void Physics::AddConeEnvironment(PhysicsObj*& argPhysObj,
		float_t argPosX, float_t argPosY, float_t argPosZ,
		float_t argRadius, float_t argHeight,
		float_t argMass, std::string argIdString)
	{
		assert(argPhysObj);

		btTransform _transform;
		_transform.setIdentity();
		_transform.setOrigin(btVector3(argPosX, argPosY, argPosZ));

		btConeShape* _coneShape = new btConeShape(argRadius, argHeight);

		btVector3 _inertia(0, 0, 0);
		_coneShape->calculateLocalInertia(argMass, _inertia);
		btMotionState* _motion = new btDefaultMotionState(_transform);

		btRigidBody::btRigidBodyConstructionInfo _info(argMass, _motion, _coneShape, _inertia);
		argPhysObj->body = new btRigidBody(_info);

		// Store depth of this physics object
		argPhysObj->depth = argPosX;
		// Store the id string
		argPhysObj->idString = argIdString;

		argPhysObj->body->setUserPointer(argPhysObj);

		m_world->addRigidBody(argPhysObj->body);
	}

}